
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration
import time  
from math import sqrt  


distance_threshold=0.5   # 当到达距离目标点0.5时
def distance_to_goal(point1,point2): 
    return sqrt((point1[0]-point2[0])**2+(point1[1]-point2[1])**2)
    
def navigation2pose(navigator,point=(0.0,0.0)):  # 导航到点
    goal_pose = PoseStamped()
    goal_pose.header.frame_id = 'map'
    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose.pose.position.x = point[0]
    goal_pose.pose.position.y = point[1]
    navigator.goToPose(goal_pose)  
    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()             
        current_position = (feedback.current_pose.pose.position.x, feedback.current_pose.pose.position.y)  
        d=distance_to_goal(current_position, point)
        if  d < distance_threshold:
            #即将到达目标点时，如果超时没有到达，取消当前目标点的导航        
            if Duration.from_msg(feedback.navigation_time) > Duration(seconds=5.0):
                print("超时未到达目标点，取消任务")
                navigator.cancelTask() 


def two_points(navigator):
    # ---------------------两点巡航--------------------
    i=0
    goal_flag=1  
    while i<10:   
        if goal_flag==1:       
            navigation2pose(navigator,point=(0.0,0.0))                         
            goal_flag=2  
            i+=1                      
        if goal_flag==2:                 
            navigation2pose(navigator,point=(0.0,2.0)) 
            goal_flag=1
            i+=1 
         
 
   

def main():
    rclpy.init()
    navigator = BasicNavigator()     
    navigator.waitUntilNav2Active()


    decision_points=[
    (0.0, 0.0),   # （初始化位姿）
    (-1.48, -0.4),   #第一个导航点 
    ( 1.48, -0.95) ,     # 第二个导航点  
     ( 0.0, 2.0) ,   # 第三个导航点 
    ]    
   
    
     # 先初始化位姿  
    initial_pose = PoseStamped()
    initial_pose.header.frame_id = 'map'
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    initial_pose.pose.position.x = decision_points[0][0]
    initial_pose.pose.position.y = decision_points[0][1]
    initial_pose.pose.orientation.w = 1.0
    navigator.setInitialPose(initial_pose)
    
    
    print("去第一个目标点")
    navigation2pose(navigator,point=(decision_points[1][0],decision_points[1][1]))
    print("去第二个目标点")
    navigation2pose(navigator,point=(decision_points[2][0],decision_points[2][1]))
    print("去第三个目标点")
    navigation2pose(navigator,point=(decision_points[3][0],decision_points[3][1]))
    
    
    # 根据条件，继续导航，做出决策
    first=0
    while True:                        
        if first==0:
            print("去第一个目标点")
            navigation2pose(navigator,point=(decision_points[1][0],decision_points[1][1])) 
            first=1
        if first==1:
            print("去第二个目标点")
            navigation2pose(navigator,point=(decision_points[2][0],decision_points[2][1])) 
            first=2
        if first==2:
            print("去第三个目标点")
            navigation2pose(navigator,point=(decision_points[3][0],decision_points[3][1])) 
            first=3
        else:    
            print("去初始点")
            navigation2pose(navigator,point=(decision_points[0][0],decision_points[0][1])) 
            time.sleep(5)

    rclpy.shutdown()
 
main()

